/***************************************************************************
				DcaIntegrator.cpp
                               -------------------
Copyright (c) 2009-2010 
Author: Kishor D. Bhalerao (kishor8dm@gmail.com)
Contributors: 

This file is associated with the Ph.D. dissertation,"ON METHODS FOR EFFICIENT
AND ACCURATE DESIGN AND SIMULATION OF MULTIBODY SYSTEMS", Bhalerao K.D,
Rensselaer Polytechnic Institute, 2010.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1.) Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2.) Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

3.) The name of the authors, their employers, or sponsoring organizations may
not be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHORS, THEIR EMPLOYERS, OR SPONSORING ORGANIZATIONS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#include "DcaIntegrator.h"
#include "SimTKmolmodel.h"
#include "SimTKsimbody_aux.h"
#include <fstream>
#include <ostream>
#include <cstdlib>
#include <cmath>
#include "List.h"
#include "Assembly.h"

using namespace SimTK;
using namespace std;

extern mCon PresMo1;
extern mCon PresMo2;
extern mCon PresMo3;

void DcaIntegrator::StepTo(const Real& FinalTime,const Real& h,const bool& animate, const int& animateEveryNSteps){
	
	// This is the list to store the state vector for the entire
	// width of the moving window.
	RSIM::List <SimTK::Vector> L;	// note: typedef Vector_<Real> 	
	                                // When we assign an object to the class, we need to define the type 
	                                // of template variable; here the template variable is SimTK::Vector
	                                // and the object's name is L.
	
	// Initially all the joints are active
	MBS_.initializeActiveJointList();
		
	// For animation.
	//VTKVisualizer display(MBS_.getSystem());	
	
	State& state = MBS_.getState();	
	SimbodyMatterSubsystem& matter = MBS_.getMatter();
	MultibodySystem& system = MBS_.getSystem();
	

	bool Initialized = false;    // this initialization lets the calculation of the moving-window 
			                     // average and standard deviation start
	bool FirstInitialization = true; // This lets the calculation of the moving-window average
			                         // and standard deviation in the first window be implemented
			                         // using the regular formulation, but after that it will be 
			                         // false and the claculation is performed using recursive formulae
	
	Vector Qmean(state.getNQ()); // creates a vector whose number of elements is getNQ
	
	Qmean = 0.0; 
	Vector QStdDev(state.getNQ());
	Real currentTime = 0.0;
		
	// This data is requried just for trial runs. 
	// No need to write the mean and standard deviation to output files
	
	// This is very inconvenient and needs to be rewritten at some point.
	// DcaIntegrator should support something like a periodiceventreporter
	// similar to the one in SimTK modules. This is a quick and dirty
	// implementation which needs to be cleaned up.
	
	ofstream RBForces;
	ofstream CM;
	ofstream myFile;
	ofstream opFile;
	ofstream devFile;
	ofstream JointIDS;
	ofstream Energy;
	ofstream Temp;


    // To store the Rigid Body Forces At the Origin of the body frame of the each body
	//RBForces.open("RBForces.txt",ios::app);

	// To store the Centers of Mass of the each body
	//CM.open("CM.txt",ios::app);
	
	// To store the Q's of the system
	myFile.open("output\\Q.txt",ios::app);
	
	// To store the mean of the moving window
	opFile.open("output\\Qm.txt",ios::app);
	
	// To store the Standard deviation of the moving window
	devFile.open("output\\Qd.txt",ios::app);

	JointIDS.open("output\\JT.txt",ios::app);

	
	// This is the main integration loop
	int dispCounter = 0;
	while(currentTime<=FinalTime){	
		/*cout<< "Current time = " << currentTime <<endl;
		if(animate == true){
			++dispCounter;
			if(dispCounter == animateEveryNSteps){		
				display.report(state);			
				dispCounter=0;								
			}
		}*/
		
		
		// The method is initialized if total elements in the list
		// are greater than the windowlength provided by the user
		if(MBS_.releasedJointduringAssembly()==false){
			//std::cout <<  "nNode = "  << L.getnNodes() << "\n";
			if(L.getnNodes() >= WindowLength_ - 1) // I aded -1 
				Initialized = true;
			
		}
		else{
			cout<<"\nDeleting data in list. Recollecting the values of mean and standard deviation\n";
			
			L.clear();
			MBS_.releasedJointduringAssembly()=false;
			Initialized = false; // this initialization lets the calculation of the moving-window 
			                     // average and standard deviation start
			FirstInitialization = true; // This lets the calculation of the moving-window average
			                            //  in the first window be implemented
			                            // using the regular formulation, but after that it will be 
			                            // false and the claculation is performed using recursive formulae
		}
		
		// Store current state in the list
		L.addNode(state.getQ());
		//std::cout <<  "nNode = "  << L.getnNodes() << "\n";
							
		// Calculate mean and standard deviation of the angles
		if(Initialized==true){
			//cout << "Current time = " << currentTime << "\n";
			//cout << "Q = " << state.getQ() << "\n";
			//cout << "Calculate statistics" << "\n" ;
			// This is the calculation of the first mean
			// Sort of a seeding operation
			if(FirstInitialization==true){
				
				for(int i=0;i<WindowLength_;++i)
					Qmean += L(i);		// L(i): operator to get the Q's of each node		
				
				Qmean = Qmean/WindowLength_;
				//cout << " Qmean = " << Qmean << "\n" << "\n";
				FirstInitialization = false;
				calcStandardDeviation(L,Qmean,QStdDev);
				
				opFile<<state.getTime()<<"\t";
				devFile<<state.getTime()<<"\t";
				for (int i=0;i<state.getNQ();++i){
					opFile<<Qmean(i)<<"\t";
					devFile<<QStdDev(i)<<"\t";
				}
				opFile<<endl;
				devFile<<endl;
			}
			else{
				Qmean += (L(WindowLength_)-L(0))/WindowLength_;
				//cout << " Qmean = " << Qmean << "\n" << "\n";
				L.deleteRoot();
				
				// This operation can be improved, Need to write 
				// a recursive relation for the standard deviation
				// here instead of using the same function as before
				calcStandardDeviation(L,Qmean,QStdDev);
				
// 				opFile<<state.getTime()<<"\t";
// 				devFile<<state.getTime()<<"\t";
// 				for (int i=0;i<state.getNQ();++i){
// 					opFile<<Qmean(i)<<"\t";
// 					devFile<<QStdDev(i)<<"\t";
// 				}
// 				
// 				opFile<<endl;
// 				devFile<<endl;
			}
		}	

		// Find out which joints need to be locked
		// Note that first body has 7 generalized coordiates
		// associated with it. 4 Euler paramters and 3 center of mass
		// absolute coordinates. Consequently, JointId 1 correspondes to 
		// Q(7)
		bool JointTransition = false;
		if(Initialized==true){	
			for(int i=0;i<matter.getNumBodies()-2;++i){
				int JointId = i+1;
				
				// If the joint is already locked, no need to check for the 
				// Standard deviation of that joint. Once the joint
				// is locked, it is expected that the Q would be fixed.
				if(MBS_.getJointStatus(JointId)==-1)
					continue;
				else{
					if(QStdDev(i+7)<StdDevThreshold_){
						cout<<"\nLocked joint between Mobilized body Index:"<<i+1<<" and "<<i+2<<endl;
						MBS_.lockJointTrans(JointId);
						JointTransition = true;
// 						char ch;
// 						cin>>ch;
					}
				}
			}
			if(JointTransition){
				MBS_.projectMomenta();
				
				// This has to be called since, projectMomenta changes the value of state
				// and the Stage has to be further realized to velocity to proceede with dynamics
				system.realize(state,Stage::Velocity);
				JointTransition = false;
			}

		}
		
		//cout << "Number of Bodies = " << system.getMatterSubsystem().getNumBodies();
		// Take a step

				/*ConstraintForce F ;
		F = MBS_.getConstraintForce();
		Vec6 * F1 = F.getFcH1Ptr();
		Vec6 * F2 = F.getFcH2Ptr();
		cout << *  F1 ;
		cout << *  F2 ;*/

	   //DcaIntegrator::writeMobilityForces();
	   // Added By Ali Poursina
       // write Rigid Body Forces to the file
		//DcaIntegrator::writeRBForces(RBForces);	
		//RBForces<<endl;

		// Added By Ali Poursina
		// write CM to the file
		//DcaIntegrator::writeCM(CM);	
		//CM << CM<<endl;
		
		// write Q to the file
		DcaIntegrator::writeQ(myFile);	
		myFile<<endl;

		/*JointIDS<<state.getTime()<<"\t";
		MBS_.writeFreeJointList(JointIDS);
		JointIDS<<endl;*/
        
		system.realize(state,Stage::Dynamics);
		
		 
        
	    std ::cout << state.getTime() << std::endl;
	    Real KE = matter.calcKineticEnergy(state);
	    Real PE = system.calcPotentialEnergy(state);
	    Energy.open ( "output\\Energy.txt", ios::app);
	    Energy << state.getTime() << "\t" << KE <<  "\t" << PE << "\t" << KE + PE << std::endl;
	    Energy.close();


	    Temp.open ( "output\\Temperature.txt", ios::app);
	    Temp << state.getTime() << "\t" << Therm.getCurrentTemperature(state)  << std::endl;
		Temp.close();

		ofstream U;
		U.open ( "output\\U.txt", ios::app);
        U << state.getTime() << "\t" ;
		for (int i = 0 ; i < state.getNU() ; i++ ){
	    U << "\t" << state.getU()[i]  << "\t";
		}
		U << std::endl;
		U.close();

		//Vector V = state.getU();
	 //   //std::cout << V << std::endl;
	 //   Vector MV ;
  //      matter.calcMV(state,V,MV);
	 //   //std::cout << state_.getTime()<< std::endl; 
		//
	 //   Real Z0 = state.getZ()[1];
	 //   Vector ThermGeneralizedForce = -Z0 * MV; 

		/*ofstream Z;
		Z.open ( "Z0.txt", ios::app);
	    Z << state.getTime() << "\t" << Z0  << std::endl;
		Z.close();*/

		/*ofstream Zdot;
		Zdot.open ( "ZDot.txt", ios::app);
	    Zdot << state.getTime() << "\t" << state.getZDot()[0] << "\t" << state.getZDot()[1] << "\t"  << state.getZDot()[2] << std::endl;
		Zdot.close();*/

		/*ofstream TempGenForce;
		TempGenForce.open ( "TempGenForce.txt", ios::app);
		TempGenForce << state.getTime() << "\t" << std::endl;
		for (int i = 0 ; i < state.getNU() ; i++ ){
		TempGenForce << "\t" << ThermGeneralizedForce[i]  << "\t";
		}
		TempGenForce.close();*/

		/*std::cout << Therm.getNumThermalDofs(state) << std::endl ; 
		std::cout << Therm.getNumExcludedDofs(state) << std::endl ; */

	   filebuf fb;
       fb.open ("output\\Trajectory.pdb",ios::app);
       ostream os(&fb);
	   os << "HEADER" << std::endl;
	   rna.writePdb(state,os,Transform(Vec3(0)));
       os << "END" << std::endl;

	

		//this->SingleStepRK4(currentTime,h);
		this->SingleStepVerlet(currentTime,h);
		currentTime +=  h;	
		

	} // This is the end of the integration loop
	
	// Close files
	//RBForces.close();
	//CM.close();
	myFile.close();
	opFile.close();
	devFile.close();

}

void DcaIntegrator::calcStandardDeviation(const RSIM::List <SimTK::Vector>& L, const Vector& Qmean, Vector &QStdDev){
	
	State& state = MBS_.getState();
	int nq = state.getNQ();
	Vector Qdiff(nq);
	Vector tmp(nq);
	
	Qdiff = 0.0;
		
	double len = L.Length();
		
	for(int i=0;i<len;++i){
		tmp = L.getNode(i)-Qmean;		
		for(int j=0;j<nq;++j){
			Qdiff(j) += tmp(j)*tmp(j);
		}
		
	}
	for(int j=0;j<nq;++j){
		QStdDev(j) = sqrt(Qdiff(j)/len);
	}		
}

void DcaIntegrator::SingleStepVerlet(const Real& time, const Real& h){	
	State& state = MBS_.getState();
	MultibodySystem& system = MBS_.getSystem();

	

	state.updTime() = time;
	
	
	// Set position and velocity level constraints on first body
	//state.updQ()[4] = PresMo1.f(time); // set x coordinate
	//state.updQ()[5] = PresMo2.f(time); // set y coordinate
	//state.updQ()[6] = PresMo3.f(time); // set z coordinate
	//
	//state.updU()[3] = PresMo1.fdot(time); // set xdot 
	//state.updU()[4] = PresMo2.fdot(time); // set ydot
	//state.updU()[5] = PresMo3.fdot(time); // set zdot


	// At this step by giving the Q's and U's to simTK, the system is realized to the velocity level.
	// Therefore, all the kinematics, (angular velocities and velocities of the stations ,...
	// are calculated using the tools in simtk
	system.realize(state,Stage::Velocity);
    int nu = state.getNU();
	int nq = state.getNQ();


	// This is added to calculate Z from Zdot, the second element of Z is Z[1] which is used to find the 
	// mobility force from the thermostat -Z[1] * M * U
	int nz = state.getNZ();

	
	Vector qdot = state.getQDot();
	const Vector u0 = state.getU(); // The value of the U's at t, we need to keep this value to find 
	                                // U at t + h
    const Vector z0 = state.getZ(); // The value of the Z's at t, we need to keep this value to find 
	                                // Z at t + h
	//std::cout << z0 << std::endl;
	//std::cout << state.getU() << std::endl;
	//std::cout << state.getQ() << std::endl;

	
	
	Vector Udot(nu);
	Vector Qdotdot(nq);

	//std::cout << "Q  " << state.getQ() << std::endl << std::endl;;
	//std::cout << "U  " << state.getU() << std::endl << std::endl;;
	MBS_.calcUdot(Udot); //all the Udots are calculated using DCA for time t from all the q's and u's at t.
	fstream Out;
	Out.open("output\\UDot.txt",ios::app);
	Out << state.getTime() << "\t" ;
	for (int i = 0 ; i < state.getNU() ; i++ ){
	    Out << "\t" << Udot[i] << "\t";
	}
	Out << std::endl ;
	Out.close();


	//std::cout << Udot << std::endl;
	
	
	MBS_.calcQdotdotFromUdot(Qdotdot,Udot);
	const Vector udot0 = Udot;
	system.realize(state,Stage::Dynamics);
	const Vector zdot0 = state.getZDot(); //all the Zdots at time t are calculated using simtk for time t from all the q's and u's of t
	//std::cout << "Zdot  " << state.getZDot() << std::endl << std::endl;;
	
	//std::cout << state.getTime() << std::endl;
    //std::cout << state.getQ() << std::endl;

	// Added by Sherm: Update the time at which Udot are used to calculate q(t+h) and u(t+h)

	state.updTime() = time+h;

	// This value will remain fixed and will not change
	// This is the value of the q for the next time step (t+h), it must be fixed and never changes
	state.updQ() = state.getQ() + qdot*h + 0.5*Qdotdot*h*h;
	//state.updQ()[4] = PresMo1.f(time+h); // set x coordinate
	//state.updQ()[5] = PresMo2.f(time+h); // set y coordinate
	//state.updQ()[6] = PresMo3.f(time+h); // set z coordinate
	
	//std::cout<<state.getU() << std::endl; //  line **

	// Get a first estimate of u and Z at time t+h, this estimate is used in the iterations to find u(t+h) and z(t+h)
	state.updU() = u0 + Udot*h; //Udot from DCA at t
	state.updZ() = z0 + zdot0*h;//Zdot from simtk at t
	
	//Real U3 = PresMo1.fdot(time+h); 
	//Real U4 = PresMo2.fdot(time+h);
	//Real U5 = PresMo3.fdot(time+h);
	//state.updU()[3] = U3; // set xdot 
	//state.updU()[4] = U4; // set ydot
	//state.updU()[5] = U5; // set zdot


	//std::cout<<state.getU() << std::endl; // To see if the U's are updated compare the value with line **
	// trapezoid rule: u(t+h) = u(t) + h*(f(u(t))+f(u(t+h)))/2.  
	// Similarly: Z(t+h) = Z(t) + h*(f(Z(t))+f(Z(t+h)))/2.   
	const Real tol = 1e-7;
	const Real MaxIterations = 10;
	const Real TinyReal = 0.00001;
	Vector usave(nu);
	Vector zsave(nz);
	bool converged = false;
	
	

	// Hint: Before starting the iteration loop: the q's we are using are q(t+h)
	                                           // The U's at this step are the estimated one: u0 + Udot*h
	                                           // The Z's at this step are the estimated one: z0 + zdot*h
	for(int i = 0; !converged && i< MaxIterations; i++){
		//Dont need "system.realize(state,Stage::Velocity); // q is q(t+h), u and z are the estimated ones"
		// since we realize the state to the dynamics further in line ***
		usave = state.getU();	// old estimated U's
		zsave = state.getZ();   // old estimated Z's
		
		MBS_.calcUdot(Udot); // using DCA to find udot based on fixed q(t+h) and non-fixed old U_estimated
		

		system.realize(state,Stage::Dynamics); // using simTk to find Zdot based on fixed q(t+h) and non-fixed old U_estimated // line ***
		state.setZ(z0 + 0.5*h*(zdot0 + state.getZDot())); // These are new estimated Z's

		state.setU(u0 + 0.5*h*(udot0 + Udot)); // These are new estimated U's
		//state.updU()[3] = U3; // set xdot 
		//state.updU()[4] = U4; // set ydot	
		//state.updU()[5] = U5; // set zdot
        
		//std::cout << "U= "<< state.getU()<<std::endl;
		//std::cout << "Z= "<< state.getZ()<<std::endl; 
        
     	const Real convergenceU = (state.getU()-usave).norm()/(usave.norm()+TinyReal); // difference between old and new esimated U's
		const Real convergenceZ = (state.getZ()-zsave).norm()/(zsave.norm()+TinyReal); // difference between old and new esimated Z's
		const Real change = std::max(convergenceU,convergenceZ);
		converged = (change <= tol);
		if (i == MaxIterations){
			cout<<"\nExceeded the total number of iterations\n..";
			cout<<"\nEnter character to exit";
			cin.get();
			exit(0);
		}
	}
}


void DcaIntegrator::SingleStepRK4(const Real& time, const Real& h){
	
	State& state = MBS_.getState();
	MultibodySystem& system = MBS_.getSystem();

	state.updTime() = time;
	
	// Set position and velocity level constraints on first body
	state.updQ()[4] = PresMo1.f(time); // set x coordinate
	state.updQ()[5] = PresMo2.f(time); // set y coordinate
	state.updQ()[6] = PresMo3.f(time); // set z coordinate
		
	state.updU()[3] = PresMo1.fdot(time); // set xdot 
	state.updU()[4] = PresMo2.fdot(time); // set ydot
	state.updU()[5] = PresMo3.fdot(time); // set zdot
	
	system.realize(state,Stage::Velocity); // when we realized the system at Velocity level, all the stages
	                                       // prior to acceleration are realized

	int nu = state.getNU();

	const Vector U = state.getU();
	const Vector Q = state.getQ();

	Vector Udot(nu);

	// Evaluate k1=f(t,y)               
	MBS_.calcUdot(Udot);
	Vector K1dot = Udot;
	Vector K1 = state.getQDot();

	// Evaluate K2 = f(t+0.5*deltaT,y+0.5*deltaT*K1)
	state.setQ(Q+K1*0.5*h);
	state.setU(U+K1dot*0.5*h);
	
	// Set position and velocity level constraints on first body
	state.updQ()[4] = PresMo1.f(time+0.5*h); // set x coordinate
	state.updQ()[5] = PresMo2.f(time+0.5*h); // set y coordinate
	state.updQ()[6] = PresMo3.f(time+0.5*h); // set z coordinate
	
	state.updU()[3] = PresMo1.fdot(time+0.5*h); // set xdot 
	state.updU()[4] = PresMo2.fdot(time+0.5*h); // set ydot
	state.updU()[5] = PresMo3.fdot(time+0.5*h); // set zdot

	system.realize(state,Stage::Velocity);

	MBS_.calcUdot(Udot);
	Vector K2dot = Udot;
	Vector K2 = state.getQDot();

	// Evaluate K3 = f(t+0.5*deltaT,y+0.5*deltaT*K2)
	state.setQ(Q+K2*0.5*h);
	state.setU(U+K2dot*0.5*h);
	
	// Set position and velocity level constraints on first body
	state.updQ()[4] = PresMo1.f(time+0.5*h); // set x coordinate
	state.updQ()[5] = PresMo2.f(time+0.5*h); // set y coordinate
	state.updQ()[6] = PresMo3.f(time+0.5*h); // set z coordinate
	
	state.updU()[3] = PresMo1.fdot(time+0.5*h); // set xdot 
	state.updU()[4] = PresMo2.fdot(time+0.5*h); // set ydot
	state.updU()[5] = PresMo3.fdot(time+0.5*h); // set zdot

	system.realize(state,Stage::Velocity);

	MBS_.calcUdot(Udot);
	Vector K3dot = Udot;
	Vector K3 = state.getQDot();

	// Evaluate K4 = f(t+deltaT,y+deltaT*K3)
	state.setQ(Q+h*K3);
	state.setU(U+h*K3dot);
	
	// Set position and velocity level constraints on first body
	state.updQ()[4] = PresMo1.f(time+h); // set x coordinate
	state.updQ()[5] = PresMo2.f(time+h); // set y coordinate
	state.updQ()[6] = PresMo3.f(time+h); // set z coordinate
	
	state.updU()[3] = PresMo1.fdot(time+h); // set xdot 
	state.updU()[4] = PresMo2.fdot(time+h); // set ydot
	state.updU()[5] = PresMo3.fdot(time+h); // set zdot

	system.realize(state,Stage::Velocity);

	MBS_.calcUdot(Udot);
	Vector K4dot = Udot;
	Vector K4 = state.getQDot();

	Vector Qnew = Q + h*(K1+2*K2+2*K3+K4)/6;
	Vector Unew = U + h*(K1dot+2*K2dot+2*K3dot+K4dot)/6;

	state.setQ(Qnew);
	state.setU(Unew);
	
	// Set position and velocity level constraints on first body
	state.updQ()[4] = PresMo1.f(time+h); // set x coordinate
	state.updQ()[5] = PresMo2.f(time+h); // set y coordinate
	state.updQ()[6] = PresMo3.f(time+h); // set z coordinate
	
	state.updU()[3] = PresMo1.fdot(time+h); // set xdot 
	state.updU()[4] = PresMo2.fdot(time+h); // set ydot
	state.updU()[5] = PresMo3.fdot(time+h); // set zdot
}

// void DcaIntegrator::writePDB(std::ostream& OutputStream){
// 	CompoundSystem::CompoundIndex c(0);
// 	DcaIntegrator::MBS_.getSystem().getCompound(c).writePdb(MBS_.getState(),OutputStream,Transform(Vec3(0)));
// }

void DcaIntegrator::writeQ(std::ostream& OutputStream){
	State& state = MBS_.getState();
	//MultibodySystem& system = MBS_.getSystem();
	//system.realize(state,Stage::Dynamics);
	//Real TotEnergy = system.calcEnergy(state);
	OutputStream<<state.getTime()<<"\t";
		for(int i=0;i<state.getNQ();++i)
		OutputStream<<setw(8)<<state.getQ()[i]<<"\t";
	//OutputStream<<TotEnergy;
}

// Added By Ali Poursina
void DcaIntegrator::writeCM(std::ostream& OutputStream){
	State& state = MBS_.getState();
	MultibodySystem& system = MBS_.getSystem();
	int NBody = system.getMatterSubsystem().getNumBodies();
	system.realize(state,Stage::Position);
	//Real TotEnergy = system.calcEnergy(state);
	OutputStream<<state.getTime()<<"\t";
	for(int i=0;i<NBody-1;++i){
		MobilizedBody body = system.getMatterSubsystem().getMobilizedBody(MobilizedBodyIndex(i+1));
	    Vec3 pos = body.getBodyOriginLocation(state);
       // std::cout<<"\t"<<pos[0]<<"\t"<<pos[1]<<std::endl;
		OutputStream<<setw(8)<<pos[0]<<"\t"<<pos[1]<<"\t"<<pos[2]<<"\t" ;
	}
	//OutputStream<<TotEnergy;
}


// Added By Ali Poursina
void DcaIntegrator::writeRBForces(std::ostream& OutputStream){
	State& state = MBS_.getState();
	MultibodySystem& system = MBS_.getSystem();

	system.realize(state,Stage::Dynamics);
	SimbodyMatterSubsystem& matter = MBS_.getMatter();
	//const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(1));
    
	OutputStream<<state.getTime()<<"\t";

	SpatialVec RigidBodyForce;
    int NBody = system.getMatterSubsystem().getNumBodies();
	for(int i=0;i<NBody-1;++i){
		RigidBodyForce = system.getRigidBodyForces(state,Stage::Dynamics)[i+1] ;
	 	//OutputStream<<setw(8)<<RigidBodyForce(0)<<"\t"<<RigidBodyForce(1) << "\t";
		cout<<setw(8)<<RigidBodyForce(0)<<"\t"<<RigidBodyForce(1) << "\t";
	}
	
}

//Vector MobilityForce;
//   // int NBody = system.getMatterSubsystem().getNumBodies();
//	for(int i=0;i<NBody-1;++i){
//		MobilityForce = system.getMobilityForces(state,Stage::Dynamics) ;
//	 	//OutputStream<<setw(8)<<RigidBodyForce(0)<<"\t"<<RigidBodyForce(1) << "\t";
//		cout<<setw(8)<<system.getMobilityForces(state,Stage::Dynamics)<< endl  ;
//	}

//Added By Ali Poursina
void DcaIntegrator::writeMobilityForces(){
	State& state = MBS_.getState();
	MultibodySystem& system = MBS_.getSystem();

	system.realize(state,Stage::Dynamics);
	SimbodyMatterSubsystem& matter = MBS_.getMatter();
	  
	
	Vector MobilityForce;
    int NBody = system.getMatterSubsystem().getNumBodies();
	//for(int i=0;i<NBody-1;++i){
		MobilityForce = system.getMobilityForces(state,Stage::Dynamics) ;
	 	//OutputStream<<setw(8)<<RigidBodyForce(0)<<"\t"<<RigidBodyForce(1) << "\t";
		cout<<setw(8)<<MobilityForce << "\t";
	//}
	
}